{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "from sympy import symbols, cos, sin, pi, simplify, pprint, tan, expand_trig, sqrt, trigsimp, atan2\n",
    "from sympy.matrices import Matrix"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# rotation matrices in x, y, z axes\n",
    "\n",
    "def rotx(q):\n",
    "\n",
    "  sq, cq = sin(q), cos(q)\n",
    "\n",
    "  r = Matrix([\n",
    "    [1., 0., 0.],\n",
    "    [0., cq,-sq],\n",
    "    [0., sq, cq]\n",
    "  ])\n",
    "    \n",
    "  return r\n",
    "\n",
    "\n",
    "def roty(q):\n",
    "\n",
    "  sq, cq = sin(q), cos(q)\n",
    "\n",
    "  r = Matrix([\n",
    "    [ cq, 0., sq],\n",
    "    [ 0., 1., 0.],\n",
    "    [-sq, 0., cq]\n",
    "  ])\n",
    "    \n",
    "  return r\n",
    "\n",
    "\n",
    "def rotz(q):\n",
    "\n",
    "  sq, cq = sin(q), cos(q)\n",
    "\n",
    "  r = Matrix([\n",
    "    [cq,-sq, 0.],\n",
    "    [sq, cq, 0.],\n",
    "    [0., 0., 1.]\n",
    "  ])\n",
    "    \n",
    "  return r"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def pose(theta, alpha, a, d):\n",
    "  # returns the pose T of one joint frame i with respect to the previous joint frame (i - 1)\n",
    "  # given the parameters:\n",
    "  # theta: theta[i]\n",
    "  # alpha: alpha[i-1]\n",
    "  # a: a[i-1]\n",
    "  # d: d[i]\n",
    "\n",
    "  r11, r12 = cos(theta), -sin(theta)\n",
    "  r23, r33 = -sin(alpha), cos(alpha)\n",
    "  r21 = sin(theta) * cos(alpha)\n",
    "  r22 = cos(theta) * cos(alpha)\n",
    "  r31 = sin(theta) * sin(alpha)\n",
    "  r32 = cos(theta) * sin(alpha)\n",
    "  y = -d * sin(alpha)\n",
    "  z = d * cos(alpha)\n",
    "    \n",
    "  T = Matrix([\n",
    "    [r11, r12, 0.0, a],\n",
    "    [r21, r22, r23, y],\n",
    "    [r31, r32, r33, z],\n",
    "    [0.0, 0.0, 0.0, 1]\n",
    "  ])\n",
    "  \n",
    "  T = simplify(T)\n",
    "\n",
    "  return T"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "# get the pose (homogenous transforms) of each joint wrt to previous joint\n",
    "\n",
    "q1, q2, q3, q4, q5, q6= symbols('q1:7')\n",
    "\n",
    "d90 = pi / 2 \n",
    "\n",
    "T01 = pose(q1, 0, 0, 0.75)\n",
    "T12 = pose(q2 - d90, -d90, 0.35, 0)\n",
    "T23 = pose(q3, 0, 1.25, 0)\n",
    "T34 = pose(q4, -d90, -0.054, 1.5)\n",
    "T45 = pose(q5, d90, 0, 0)\n",
    "T56 = pose(q6, -d90, 0, 0)\n",
    "T6g = pose(0, 0, 0, 0.303)\n",
    "\n",
    "T0g_a = simplify(T01 * T12 * T23 * T34 * T45 * T56 * T6g)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Matrix([\n",
      "[1.0*sin(alpha)*sin(gamma) + sin(beta)*cos(alpha)*cos(gamma),  1.0*sin(alpha)*cos(gamma) - 1.0*sin(beta)*sin(gamma)*cos(alpha), 1.0*cos(alpha)*cos(beta), px],\n",
      "[sin(alpha)*sin(beta)*cos(gamma) - 1.0*sin(gamma)*cos(alpha), -1.0*sin(alpha)*sin(beta)*sin(gamma) - 1.0*cos(alpha)*cos(gamma), 1.0*sin(alpha)*cos(beta), py],\n",
      "[                                   1.0*cos(beta)*cos(gamma),                                        -1.0*sin(gamma)*cos(beta),           -1.0*sin(beta), pz],\n",
      "[                                                          0,                                                                0,                        0,  1]])\n"
     ]
    }
   ],
   "source": [
    "# Total transform wrt gripper given\n",
    "# yaw (alpha), pitch (beta), roll (beta)\n",
    "# position px, py, pz\n",
    "\n",
    "px, py, pz = symbols('px py pz', real = True)\n",
    "alpha, beta, gamma = symbols('alpha beta gamma', real = True)\n",
    "\n",
    "R = rotz(alpha) * roty(beta) * rotx(gamma) * (rotz(pi) * roty(-pi/2)).T\n",
    "\n",
    "T0g_b = Matrix([\n",
    "  [R[0, 0], R[0, 1], R[0, 2], px],\n",
    "  [R[1, 0], R[1, 1], R[1, 2], py],\n",
    "  [R[2, 0], R[2, 1], R[2, 2], pz],\n",
    "  [0, 0, 0, 1]\n",
    "])\n",
    "\n",
    "T0g_b = simplify(trigsimp(T0g_b))\n",
    "print(T0g_b)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "⎡0.257143295038827   0.48887208255965   -0.833595473062543  0.49792⎤\n",
      "⎢                                                                  ⎥\n",
      "⎢0.259329420712765  0.796053601157403    0.54685182237706   1.3673 ⎥\n",
      "⎢                                                                  ⎥\n",
      "⎢0.93092726749696   -0.356795110642117  0.0779209320563015  2.4988 ⎥\n",
      "⎢                                                                  ⎥\n",
      "⎣        0                  0                   0             1.0  ⎦\n",
      "\n",
      "Matrix([\n",
      "[1.0*sin(alpha)*sin(gamma) + sin(beta)*cos(alpha)*cos(gamma),  1.0*sin(alpha)*cos(gamma) - 1.0*sin(beta)*sin(gamma)*cos(alpha), 1.0*cos(alpha)*cos(beta), px],\n",
      "[sin(alpha)*sin(beta)*cos(gamma) - 1.0*sin(gamma)*cos(alpha), -1.0*sin(alpha)*sin(beta)*sin(gamma) - 1.0*cos(alpha)*cos(gamma), 1.0*sin(alpha)*cos(beta), py],\n",
      "[                                   1.0*cos(beta)*cos(gamma),                                        -1.0*sin(gamma)*cos(beta),           -1.0*sin(beta), pz],\n",
      "[                                                          0,                                                                0,                        0,  1]])\n"
     ]
    }
   ],
   "source": [
    "''' \n",
    "px, py, pz = 0.49792, 1.3673, 2.4988\n",
    "roll, pitch, yaw = 0.366, -0.078, 2.561\n",
    "\n",
    "q1: 1.01249809363771\n",
    "q2: -0.275800363737724\n",
    "q3: -0.115686651053751\n",
    "q4: 1.63446527240323\n",
    "q5: 1.52050002599430\n",
    "q6: -0.815781306199679\n",
    "'''\n",
    "\n",
    "Tb = T0g_b.evalf(subs = {\n",
    "  gamma: 0.366, #roll\n",
    "  beta: -0.078, #pitch\n",
    "  alpha: 2.561, #yaw\n",
    "  px: 0.49792, \n",
    "  py: 1.3673,\n",
    "  pz: 2.4988\n",
    "})\n",
    "\n",
    "print()\n",
    "pprint(Tb)\n",
    "print()\n",
    "print(T0g_b)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "⎡0.257143295038831  0.488872082559654   -0.83359547306254   0.497920000000004⎤\n",
      "⎢                                                                            ⎥\n",
      "⎢0.259329420712762  0.796053601157401   0.546851822377065   1.36729999999999 ⎥\n",
      "⎢                                                                            ⎥\n",
      "⎢0.93092726749696   -0.356795110642117  0.0779209320563043  2.49880000000001 ⎥\n",
      "⎢                                                                            ⎥\n",
      "⎣        0                  0                   0                  1.0       ⎦\n",
      "\n",
      "Matrix([\n",
      "[((sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*cos(q5) + sin(q5)*cos(q1)*cos(q2 + q3))*cos(q6) - (-sin(q1)*cos(q4) + sin(q4)*sin(q2 + q3)*cos(q1))*sin(q6), -((sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*cos(q5) + sin(q5)*cos(q1)*cos(q2 + q3))*sin(q6) + (sin(q1)*cos(q4) - sin(q4)*sin(q2 + q3)*cos(q1))*cos(q6), -(sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*sin(q5) + cos(q1)*cos(q5)*cos(q2 + q3), -0.303*sin(q1)*sin(q4)*sin(q5) + 1.25*sin(q2)*cos(q1) - 0.303*sin(q5)*sin(q2 + q3)*cos(q1)*cos(q4) - 0.054*sin(q2 + q3)*cos(q1) + 0.303*cos(q1)*cos(q5)*cos(q2 + q3) + 1.5*cos(q1)*cos(q2 + q3) + 0.35*cos(q1)],\n",
      "[ ((sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1))*cos(q5) + sin(q1)*sin(q5)*cos(q2 + q3))*cos(q6) - (sin(q1)*sin(q4)*sin(q2 + q3) + cos(q1)*cos(q4))*sin(q6), -((sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1))*cos(q5) + sin(q1)*sin(q5)*cos(q2 + q3))*sin(q6) - (sin(q1)*sin(q4)*sin(q2 + q3) + cos(q1)*cos(q4))*cos(q6), -(sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1))*sin(q5) + sin(q1)*cos(q5)*cos(q2 + q3),  1.25*sin(q1)*sin(q2) - 0.303*sin(q1)*sin(q5)*sin(q2 + q3)*cos(q4) - 0.054*sin(q1)*sin(q2 + q3) + 0.303*sin(q1)*cos(q5)*cos(q2 + q3) + 1.5*sin(q1)*cos(q2 + q3) + 0.35*sin(q1) + 0.303*sin(q4)*sin(q5)*cos(q1)],\n",
      "[                                                                -(sin(q5)*sin(q2 + q3) - cos(q4)*cos(q5)*cos(q2 + q3))*cos(q6) - sin(q4)*sin(q6)*cos(q2 + q3),                                                                  (sin(q5)*sin(q2 + q3) - cos(q4)*cos(q5)*cos(q2 + q3))*sin(q6) - sin(q4)*cos(q6)*cos(q2 + q3),                                     -sin(q5)*cos(q4)*cos(q2 + q3) - sin(q2 + q3)*cos(q5),                                                                                 -0.303*sin(q5)*cos(q4)*cos(q2 + q3) - 0.303*sin(q2 + q3)*cos(q5) - 1.5*sin(q2 + q3) + 1.25*cos(q2) - 0.054*cos(q2 + q3) + 0.75],\n",
      "[                                                                                                                                                            0,                                                                                                                                                             0,                                                                                        0,                                                                                                                                                                                                              1]])\n"
     ]
    }
   ],
   "source": [
    "Ta = T0g_a.evalf(subs = {\n",
    "  q1: 1.01249809363771,\n",
    "  q2: -0.275800363737724,\n",
    "  q3: -0.115686651053751,\n",
    "  q4: 1.63446527240323,\n",
    "  q5: 1.52050002599430,\n",
    "  q6: -0.815781306199679    \n",
    "})\n",
    "\n",
    "print()\n",
    "pprint(Ta)\n",
    "print()\n",
    "print(T0g_a)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.5.2"
  },
  "widgets": {
   "state": {},
   "version": "1.1.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
